# -*- coding: utf-8 -*-
"""
Created on Wed Apr  6 07:35:54 2022

@author: lizhuang
"""

# -*- coding: utf-8 -*-
"""
Created on Tue Apr  5 12:59:47 2022

@author: lizhuang
"""
import sympy
import numpy as np
a,b,c,a1,b1,c1,x,y = sympy.symbols("a b c a1 bi c1 x y",real=True)
rc1=0.2
l=0.4
l2=1.2
rc2=0.6
a=(rc1*sympy.cos(x))
a1=rc1*sympy.sin(x)
b=(l*sympy.cos(x))+(rc2*sympy.cos(y))
b1=l*sympy.sin(x)+rc2*sympy.sin(y)
c=(l*sympy.cos(x))+(l2*sympy.cos(y))
c1=l*sympy.sin(x)+l2*sympy.sin(y)
funcs = sympy.Matrix([a,a1,b,b1,c,c1])
args = sympy.Matrix([x,y])
res = funcs.jacobian(args)
print(res)
d,d1,e,e1 = sympy.symbols("d d1 e e1",real=True)
rp2=0.3
d=(l*sympy.cos(x))+(rp2*sympy.cos(y))
d1=l*sympy.sin(x)+rp2*sympy.sin(y)
e=(l*sympy.cos(x))+(l2*sympy.cos(y))
e1=l*sympy.sin(x)+l2*sympy.sin(y)
funcs1 = sympy.Matrix([d,d1,e,e1])
args1 = sympy.Matrix([x,y])
res1 = funcs1.jacobian(args1)
print(res1)
g=9.8
m1=1.2
m2=3.6
m3=6
d=-(l*sympy.cos(x)/l2*sympy.cos(y))
mg=np.array([[0,-m1*g,0,-m2*g,0,-m3*g]])
print(mg.shape)
dd=mg.dot(res)
print(dd.shape)
#print(dd)
fx2=10
fy2=10
fx3=20
f=np.array([-fx2,fy2,fx3,0])
mm1=25
m=np.array([mm1,0])
dmn=np.array([[1],[d]])
print(dmn.shape)
重力作用=dd.dot(dmn)
print(重力作用)
print(重力作用.shape)
f作用=f.dot(res1)
f效果=f作用.dot(dmn)
等效力矩=f效果+重力作用+mm1
print(等效力矩)
#计算力矩=等效力矩.subs({x:2,y:2})'''
